package com.suray.wcs.service.rescontact;

import com.suray.basic.wcs.path.core.Lifter;
import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.path.enums.NodeStatus;
import com.suray.basic.wcs.path.enums.NodeType;
import com.suray.basic.wcs.plc.core.Plc;
import com.suray.basic.wcs.plc.enums.CargoState;
import com.suray.basic.wcs.plc.enums.Location;
import com.suray.basic.wcs.plc.enums.PlcDeviceType;
import com.suray.basic.wcs.plc.pojo.DeviceBase;
import com.suray.basic.wcs.plc.pojo.oper.single.SingleAutoDoor;
import com.suray.basic.wcs.plc.pojo.oper.single.TurnoverOper;
import com.suray.basic.wcs.plc.service.impl.PlcDeviceDBServiceImpl;
import com.suray.basic.wcs.plc.oper.impl.PlcOperImpl;
import com.suray.basic.wcs.plc.pojo.oper.AutomaticDoorOper;
import com.suray.basic.wcs.plc.pojo.oper.lifter.LifterOper;
import com.suray.basic.wcs.plc.pojo.oper.lifter.TwoDirectionRgvLifterOper;
import com.suray.basic.wcs.plc.utils.PlcExecuteException;
import com.suray.basic.wcs.plc.utils.PlcUnit;
import com.suray.basic.wcs.utils.*;
import com.suray.commin.core.utils.SpringContextHolder;
import com.suray.type.RollerDirectionType;
import com.suray.wcs.res.enums.Order;
import com.suray.wcs.res.enums.RgvStatus;
import com.suray.wcs.service.core.util.LockFloorManager;
import com.suray.wcs.service.core.util.LockNodeManager;
import com.suray.wcs.service.enums.DetailProcess;
import com.suray.wcs.service.enums.PrecessOperation;
import com.suray.wcs.service.enums.PrecessStatus;
import com.suray.wcs.res.enums.RgvType;
import com.suray.wcs.service.execption.OccupyLifterFailException;
import com.suray.wcs.service.execption.PalletDownException;
import com.suray.wcs.service.init.SystemInit;
import com.suray.wcs.service.plc.LifterEvent;
import com.suray.wcs.service.plc.util.LifterEventInterface;
import com.suray.wcs.service.pojo.Rgv;
import com.suray.wcs.service.pojo.RgvFork;
import com.suray.wcs.service.system.entity.NodeDB;
import com.suray.basic.wcs.plc.entity.PlcDeviceDB;
import com.suray.wcs.service.system.entity.TaskDB;
import com.suray.wcs.service.system.entity.TaskDetailDB;
import com.suray.wcs.service.system.service.impl.*;
import com.suray.wcs.service.thread.PlcTaskThread;
import com.suray.wcs.service.util.WcsUtil;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.io.IOException;
import java.util.*;

import static com.suray.basic.wcs.path.core.Node.parseGrid;

public class ActionHandle {
    private static Logger log = LoggerFactory.getLogger(ActionHandle.class);
    private static NodeDBServiceImpl nodedbServiceImpl = SpringContextHolder.getBean(NodeDBServiceImpl.class);
    private static WareHouseDBServiceImpl wcsDistrictService = SpringContextHolder.getBean(WareHouseDBServiceImpl.class);
    private static TaskDetailServiceImpl taskDetailService = SpringContextHolder.getBean(TaskDetailServiceImpl.class);
    private static PlcDeviceDBServiceImpl plcDeviceDBService = SpringContextHolder.getBean(PlcDeviceDBServiceImpl.class);
    private static RgvDBServiceImpl rgvDBService = SpringContextHolder.getBean(RgvDBServiceImpl.class);

    /**
     * 执行特定的动作
     *
     * @param taskDB    当前任务
     * @param rgv       当前车辆
     * @param startNode 执行该动作的位置
     * @param endNode   执行该动作的位置
     * @param action    需要执行的动作
     * @return
     * @throws PlcExecuteException
     * @throws IOException
     */
    public static String actionHandle(TaskDB taskDB, TaskDetailDB taskDetailDB, Rgv rgv, Node startNode, Node endNode, Action action) throws PlcExecuteException, IOException {
        Node rgvNode;
        ToRes toRes = ToRes.getInstance(rgv);
        List<Node> autoDoors;
        switch (action) {
            case WAIT_ROLLER_LOAD_OK:
                if (rgv.checkRgvType(RgvType.RGV_ROLLER)) {
                    NodeDB nodeDB = nodedbServiceImpl.getNodeDB(startNode);
                    //等待取货完成
                    toRes.waitHasCargo();
                }

                return rgv.getRgvName() + "等待取货完成";
            case WAIT_ROLLER_UNLOAD_OK:
                if (rgv.checkRgvType(RgvType.RGV_ROLLER)) {
                    NodeDB nodeDB = nodedbServiceImpl.getNodeDB(startNode);
                    //等待取货完成
                    toRes.waitNoCargo();
                }

                return rgv.getRgvName() + "等待放货完成";
            case PALLET_UP:
                rgvNode = rgv.getCurLocation();
                assert taskDB != null;
                TaskDetailDB palletUpDetail = taskDetailService.getTaskDetail(taskDB.getTaskId(), DetailProcess.PALLET_UP_INST, taskDetailDB.getUuid(), null, null,
                        PrecessStatus.CONDUCT, rgvNode.toString(), null);
                taskDB.getTaskDescription().setRgvAction("顶升中");
                if (rgv.checkRgvType(RgvType.RGV_ROLLER)) {
                    NodeDB nodeDB = nodedbServiceImpl.getNodeDB(startNode);
                    toRes.rollerRun(nodeDB.getDirection());
//                    //等待取货完成
//                    toRes.palletUp();
                } else if (rgv.checkRgvType(RgvType.RGV_FORK)) {
                    ((RgvFork) rgv).getRgvCore().action(Order.FORK_GET_CARGO,
                            nodedbServiceImpl.getNode(rgv.getCurLocation()).getForkHeight());
                } else if (rgv.checkRgvType(RgvType.RGV_SEER_COMM)) {
                    toRes.palletUp(taskDB.getTaskId());
                } else {
                    if (rgv.getRgvDB().getIsCalibration()) {
                        toRes.calibrationPalletUp(taskDB.getTaskId());
                    } else {
                        toRes.palletUp(taskDB.getTaskId());
                    }
                }
                taskDetailService.updateTaskDetailCompleteTime(palletUpDetail);
                nodedbServiceImpl.updateNodeDBBarCode(rgvNode, null);

                NodeDB startNodedb = nodedbServiceImpl.getNodeDB(rgvNode);
                updateNodeStatusN(rgvNode, startNodedb);
                // 如果是5代车，则更新小车身上有当前任务的货物
                if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL)) {
                    rgv.setCargoTaskId(taskDB.getTaskId());
                }
                log.info(rgv.getRgvName() + "执行了托盘取货，于" + rgvNode);
                return rgv.getRgvName() + "执行了托盘取货，于" + rgvNode;
            case PALLET_DOWN:
                rgvNode = rgv.getCurLocation();
                TaskDetailDB palletDownDetail = taskDetailService.getTaskDetail(taskDB.getTaskId(), DetailProcess.PALLET_DOWN_INST, taskDetailDB.getUuid(), null, null,
                        PrecessStatus.CONDUCT, rgvNode.toString(), null);
                taskDB.getTaskDescription().setRgvAction("下降中");
                if (rgv.checkRgvType(RgvType.RGV_ROLLER)) {
                    NodeDB endNodeDB = nodedbServiceImpl.getNodeDB(startNode);
                    RollerDirectionType directionType = RollerDirectionType.parseType(endNodeDB.getDirection());
                    //放货时转动方向取反
                    if (directionType == RollerDirectionType.LEFT) {
                        directionType = RollerDirectionType.RIGHT;
                    } else if (directionType == RollerDirectionType.RIGHT) {
                        directionType = RollerDirectionType.LEFT;
                    } else {
                        throw new RuntimeException("(" + endNodeDB.getGridx() + "," + endNodeDB.getGridy() + ","
                                + endNodeDB.getGridz() + ")" + "滚筒方向错误！");
                    }

                    toRes.rollerDown(directionType.getValue());
//                    //等待防货完成
//                    toRes.waitNoCargo();
                } else if (rgv.checkRgvType(RgvType.RGV_FORK)) {
                    ((RgvFork) rgv).getRgvCore().action(Order.FORK_PUT_CARGO,
                            nodedbServiceImpl.getNode(rgv.getCurLocation()).getForkHeight());
                } else {
                    toRes.palletDown(taskDB == null ? null : taskDB.getTaskId());
                }
                taskDetailService.updateTaskDetailCompleteTime(palletDownDetail);
                NodeDB endNodedb = nodedbServiceImpl.getNodeDB(rgvNode);
                // 托盘放货，终点不为空，终点是接驳点/提升机/工位验证小车是否放空货物
                checkPutPalletOk(taskDB, rgvNode, endNodedb);
                // 更新接驳点节点状态
                updateNodeStatusY(rgvNode, endNodedb);
                nodedbServiceImpl.updateNodeDBBarCode(rgvNode, taskDB != null ? taskDB.getBarCode() : "");
                // 如果是5代车，则更新小车身上无货
                if (rgv.checkRgvType(RgvType.FIVE_POINT_FOUR_TL)) {
                    rgv.setCargoTaskId(null);
                }
                log.info(rgv.getRgvName() + "执行了托盘放货，于" + rgvNode);
                return rgv.getRgvName() + "执行了托盘放货，于" + rgvNode;
            case START_CHARG:
                toRes = ToRes.getInstance(rgv);
                taskDB.getTaskDescription().setRgvAction("打开充电桩");
                toRes.openCharge(taskDB == null ? null : taskDB.getTaskId());
                rgv.setRgvStatus(RgvStatus.CHARGING);
                return rgv.getRgvName() + "执行了打开小车充电";
            case ENDCHARGE:
                toRes = ToRes.getInstance(rgv);
                if (taskDB != null) {
                    taskDB.getTaskDescription().setRgvAction("关闭充电桩");
                }
                toRes.closeCharge(taskDB == null ? null : taskDB.getTaskId());
                rgv.setRgvStatus(RgvStatus.READY);
                if (rgv.getBattery() >= rgv.getRgvDB().getMaxBatterySize()) {
                    log.info("更新{}电量达到最大充电电量时间", rgv.getRgvName());
                    rgv.getRgvDB().setBatteryFullTime(new Date());
                    rgvDBService.updateById(rgv.getRgvDB());
                }
                log.info(rgv.getRgvName() + "执行了关闭小车充电");
                return rgv.getRgvName() + "执行了关闭小车充电";
            case ASSIGN_LIFTER:
                //通过end拿到PanasonicHoistOper对象
                LifterOper hoistOper = PlcUnit.getLifterOper(new Coord(endNode));
                Lifter lifter = new Lifter(hoistOper.getCoord().getX(), hoistOper.getCoord().getY());
                rgv.setRejectStatus(Rgv.RejectStatus.WAIT);
                rgv.setRejectStatusInfo("等待提升机");
                //系统是否允许同层多车
                boolean oneFloorForMorRgv = wcsDistrictService.getMoreForOne();
                checkRgvChangeFloor(rgv, oneFloorForMorRgv);
                boolean needLock = true;
                while (needLock) {
                    // 检测是否可用
                    checkLifterIsCanLock(taskDB, rgv, startNode, endNode, hoistOper, lifter);
                    LockNodeManager.lockLifter(lifter, rgv);
                    // 不可用则解除锁定后重新去抢锁
                    if ((hoistOper.isPalletExisted(endNode.getZ(), Location.INSIDE) && rgv.isHasCargo())) {
                        LockNodeManager.unlockLifter(lifter, rgv);
                    } else {
                        needLock = false;
                    }
                }
                TaskDetailDB askDetail = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.ASK_HOIST, taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                TaskDetailDB lockDet = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.LOCK_HOIST, taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                taskDetailService.updateTaskDetailCompleteTime(lockDet);
                rgv.setRejectStatus(Rgv.RejectStatus.NONE);
                rgv.setRejectStatusInfo(null);
                LifterEventInterface lifterEvent = SystemInit.getLifterEvent(hoistOper);
                lifterEvent.rgvNotLoadAssignHoist1(hoistOper, lifter, rgv, taskDB.getPlcId(), endNode.getZ());
                taskDetailService.updateTaskDetailCompleteTime(askDetail);
                return rgv.getRgvName() + "请求" + hoistOper.getDeviceName() + "成功";
            case RGV_INSIDE_LIFTER:
                LifterOper hoistOper2 = PlcUnit.getLifterOper(new Coord(endNode));
                LifterEventInterface lifterEvent2 = SystemInit.getLifterEvent(hoistOper2);
                log.info("等待小车到达提升机内部，目的位置" + endNode);
                long sendTime1 = System.currentTimeMillis();
                TaskDetailDB waitDet = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.RUN_TO_HOIST, taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                while (!rgv.getCurLocation().equals(new Node(endNode))) {
                    rgv.checkInterrupt("ActionHandle3", sendTime1);
                    log.info("等待" + rgv.getRgvName() + "进入" + hoistOper2.getDeviceName());
                    //进提升机时不停发送提升机当前层给res
                    toRes.liftCurLayer(null, hoistOper2.isArrived() ? hoistOper2.getHoistCurrentLayer() : 0);
                    try {
                        Thread.sleep(10);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                }
                log.info(rgv.getRgvName() + "已进入" + hoistOper2.getDeviceName());
                lifterEvent2.sendShuttleArrived(hoistOper2);
                taskDetailService.updateTaskDetailCompleteTime(waitDet);
                TaskDetailDB clearDet = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.CLEAR_HOIST_TASK, taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                hoistOper2.setHoistFinalTaskNoClear();
                LifterEvent.liftWord.remove(hoistOper2.getLockLocation().toString());
                taskDetailService.updateTaskDetailCompleteTime(clearDet);
                PlcDeviceDB lifterDeviceDB2 = plcDeviceDBService.getLifterDeviceDB(hoistOper2.getPlcDeviceType().getTypeNum(), hoistOper2.getLockLocation());
                TaskDetailDB unlockDet = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.UNLOCK_HOIST, taskDetailDB.getUuid(), null, null, PrecessStatus.CONDUCT,
                        lifterDeviceDB2.getDeviceId().toString(), PrecessOperation.UNLOCK.getValue().toString());
                unlockDet.setRemark(taskDB.getPlcId().toString());
                taskDetailService.updateById(unlockDet);
                hoistOper2.unlockHoist(rgv.getRgvDB().getRgvNo());
                taskDetailService.updateTaskDetailCompleteTime(unlockDet);
                log.info(hoistOper2.getDeviceName() + "解锁成功");
                return "小车到提升机内";
            case RGV_OUTSIDE_LIFTER:
                //通过end拿到PanasonicHoistOper对象
                long sendTime2 = System.currentTimeMillis();
                TaskDetailDB finalStartDet = taskDetailDB;
                new Thread(() -> {
                    try {
                        TaskDetailDB awayDet = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.DRIVE_AWAY_HOIST, finalStartDet.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                        Lifter lifter2 = nodedbServiceImpl.getLifterByPathAction(startNode);
                        LifterOper hoistOper1 = PlcUnit.getLifterOper(new Coord(lifter2.getX(), lifter2.getY(), startNode.getZ()));
                        LifterEventInterface lifterEvent1 = SystemInit.getLifterEvent(hoistOper1);
                        log.info("等待小车驶离提升机");
                        while (rgv.getCurLocation().equals(new Node(startNode))) {
                            rgv.checkInterrupt("ActionHandle4", sendTime2);
                            if (rgv.getRgvStatus().equals(RgvStatus.ERROR)) {
                                log.info(rgv.getRgvName() + "驶离" + hoistOper1.getDeviceName() + "提升机失败，小车故障");
                                return;
                            }
                            try {
                                Thread.sleep(500);
                            } catch (InterruptedException e) {
                                e.printStackTrace();
                            }
                        }
                        lifterEvent1.sendShuttleLeave(hoistOper1);
                        taskDetailService.updateTaskDetailCompleteTime(awayDet);
                        TaskDetailDB clearDet2 = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.CLEAR_HOIST_TASK, finalStartDet.getUuid(), null, null, PrecessStatus.CONDUCT, rgv.toString(), null);
                        hoistOper1.setHoistFinalTaskNoClear();
                        LifterEvent.liftWord.remove(hoistOper1.getLockLocation().toString());
                        taskDetailService.updateTaskDetailCompleteTime(clearDet2);
                        PlcDeviceDB lifterDeviceDB = plcDeviceDBService.getLifterDeviceDB(hoistOper1.getPlcDeviceType().getTypeNum(), hoistOper1.getLockLocation());
                        TaskDetailDB unlockDet2 = taskDetailService.buildTask(taskDB.getTaskId(), DetailProcess.UNLOCK_HOIST_WCS, finalStartDet.getUuid(), null, null, PrecessStatus.CONDUCT,
                                lifterDeviceDB.getDeviceId().toString(), PrecessOperation.UNLOCK.getValue().toString());
                        unlockDet2.setRemark(rgv.getRgvDB().getRgvNo().toString());
                        taskDetailService.updateById(unlockDet2);
                        LockNodeManager.unlockLifter(lifter2, rgv);
                        hoistOper1.unlockHoist(rgv.getRgvDB().getRgvNo());
                        taskDetailService.updateTaskDetailCompleteTime(unlockDet2);
                        //系统是否允许同层多车
                        boolean moreToOne1 = wcsDistrictService.getMoreForOne();
                        // 不允许同层多车情况下
                        if (!moreToOne1) {
                            log.info("清除层锁" + rgv);
                            LockFloorManager.removeUnLocationLock(rgv);
                        }
                        log.info(hoistOper1.getDeviceName() + "解锁成功");
                        log.info(rgv.getRgvName() + "驶出提升机结束");
                    } catch (Exception e) {
                        ExceptionLogServiceImpl exceptionLogService = new ExceptionLogServiceImpl();
                        log.error(exceptionLogService.toStackTrace(e));
                    }
                }).start();
                return "开启小车驶出提升机线程";
            case START_PLC:
                PlcTaskThread.getInstance(taskDB);
                return "PLC流程开启成功" + taskDB.getTaskId() + "!";
            case ASSIGN_OPEN_DOOR:
                long sendTime3 = System.currentTimeMillis();
                autoDoors = getDoorNodes(startNode, endNode);
                Set<Coord> lockDoors = new HashSet<>();
                for (Node autoDoor : autoDoors) {
                    lockDoors.add(autoDoor);
                }
                try {
                    while (!LockNodeManager.getDeviceLockSafe(rgv, lockDoors)) {
                        rgv.checkInterrupt("锁定设备门", sendTime3);
                        WcsFuncUtil.delay(1000);
                    }
                    int tick = 10;
                    while (ParamConfig.singleAutoDoor == null) {
                        if (tick == 0) {
                            throw new RuntimeException("自动门不在线:" + WcsFuncUtil.listToString(autoDoors));
                        } else {
                            tick--;
                        }
                        WcsFuncUtil.delay(1000);
                    }
                    if (ParamConfig.singleAutoDoor) {
                        openSingleAutoDoor(rgv, taskDB, startNode, endNode);
                    } else {
                        openAutoDoorNormal(rgv, taskDB, startNode, endNode);
                    }
                } catch (Exception e) {
                    LockNodeManager.releaseDeviceLock(rgv);
                    throw e;
                }
                return rgv.getRgvName() + "请求自动门已开启";
            case ASSIGN_CLOSE_DOOR:
                int tick = 10;
                while (ParamConfig.singleAutoDoor == null) {
                    if (tick == 0) {
                        throw new RuntimeException("自动门不在线:");
                    } else {
                        tick--;
                    }
                    WcsFuncUtil.delay(1000);
                }
                if (ParamConfig.singleAutoDoor) {
                    closeSingleAutoDoor(rgv, taskDB, startNode, endNode);
                } else {
                    closeAutoDoorNormal(rgv, taskDB, startNode, endNode);
                }
                return rgv.getRgvName() + "请求自动门已关闭";
            case FORK_UP_AND_GET_CARGO:
            case FORK_PUT_CARGO:
            case FORK_GET_CARGO:
            case FORK_DOWN:
            case FORK_UP:
            case FORK_UP_BY_MOVING:
            case FORK_DOWN_BY_MOVING:
            case FORK_PUT_CARGO_AND_DOWN:
            case FORK_RUN_WITH_CARGO:
            case FORK_DOWN_TO_RUN:
            case FORK_UP_TO_RUN:
                if (rgv.checkRgvType(RgvType.RGV_FORK)) {
                    return rgv.getRgvName() + " 忽略叉车动作:" + action;
                }
            default:
                throw new RuntimeException("不支持的动作:" + action);
        }
    }

    /**
     * 关闭单体自动门
     *
     * @param rgv
     * @param taskDB
     * @param startNode
     * @param endNode
     */
    private static void closeSingleAutoDoor(Rgv rgv, TaskDB taskDB, Node startNode, Node endNode) throws IOException {
        long sendTime4 = System.currentTimeMillis();
        List<SingleAutoDoor> automaticDoorOpersClose = new ArrayList<>();
        List<TurnoverOper> turnoverOpers = new ArrayList<>();
        List<Node> autoDoors = getDoorNodes(startNode, endNode);
        while (!rgv.getCurLocation().equals(endNode)) {
            rgv.checkInterrupt("ActionHandle8", sendTime4);
            log.info(taskDB.getTaskId() + "任务等待小车到达门后");
            try {
                Thread.sleep(1000);
            } catch (Exception e) {
            }
        }
        // 终点存在且是自动门
        for (Node autoDoor : autoDoors) {
            SingleAutoDoor automaticDoorOper = (SingleAutoDoor) Plc.deviceAutomaticDoorCoordMap.get(new Coord(autoDoor));
            TurnoverOper turnoverOper = (TurnoverOper) Plc.deviceTurnoverMachineOperCoordMap.get(autoDoor);

            if (automaticDoorOper == null && turnoverOper == null) {
                WcsUtil.notification("翻转机自动门设备未配置或离线，请人工处理");
            }

            if (automaticDoorOper != null) {
                automaticDoorOpersClose.add(automaticDoorOper);
            }
            if (turnoverOper != null) {
                turnoverOpers.add(turnoverOper);
            }
            if (automaticDoorOper != null) {
                PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
                systemOperClose.lockDevice(automaticDoorOper.getDeviceNo(), rgv.getRgvDB().getRgvNo(), false);
                WcsFuncUtil.delay(100);
                systemOperClose.getPlc().refreshCache();
            }
            if (turnoverOper != null) {
                PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(turnoverOper.getPlc());
                systemOperClose.lockDevice(turnoverOper.getDeviceNo(), rgv.getRgvDB().getRgvNo(), false);
                WcsFuncUtil.delay(100);
                systemOperClose.getPlc().refreshCache();
            }
        }
        if (automaticDoorOpersClose.size() > 0 || turnoverOpers.size() > 0) {
            new Thread(() -> {
                try {
                    if (automaticDoorOpersClose.size() > 0) {
                        for (SingleAutoDoor automaticDoorOper : automaticDoorOpersClose) {
                            int waitTime = 10;
                            PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
                            while (automaticDoorOper.isLock()) {
                                WcsFuncUtil.delay(10000);
                                rgv.checkInterrupt("等待门解锁", sendTime4);
                                if (waitDeviceUnLock(automaticDoorOper, taskDB, rgv, systemOperClose, waitTime)) {
                                    break;
                                }
                            }
                        }
                    }
                    if (turnoverOpers.size() > 0) {
                        for (TurnoverOper turnoverOper : turnoverOpers) {
                            int waitTime = 10;
                            PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(turnoverOper.getPlc());
                            while (turnoverOper.isLock()) {
                                WcsFuncUtil.delay(10000);
                                rgv.checkInterrupt("等待翻转机解锁", sendTime4);
                                if (waitDeviceUnLock(turnoverOper, taskDB, rgv, systemOperClose, waitTime)) {
                                    break;
                                }
                            }
                        }
                    }
                } catch (Exception e) {
                    WcsFuncUtil.exceptionLog(e, ActionHandle.class);
                } finally {
                    LockNodeManager.releaseDeviceLock(rgv);
                }
            }).start();
        }
    }

    private static boolean waitDeviceUnLock(DeviceBase deviceBase, TaskDB taskDB, Rgv rgv, PlcOperImpl plcOper,
                                            int eventTime) {
        deviceBase.setDeviceStatus("等待解锁成功");
        if (eventTime > 0) {
            eventTime--;
            log.info(taskDB.getTaskId() + "任务, " + deviceBase.getDeviceName()
                    + "解锁失败，重发解锁");
            try {
                plcOper.lockDevice(deviceBase.getDeviceNo(), rgv.getRgvDB().getRgvNo(),
                        false);
            } catch (IOException e) {
                log.error("通讯异常,自动门操作失败!", e);
            }
            WcsFuncUtil.delay(1000);
        } else {
            WcsUtil.notification(deviceBase.getDeviceName() + "解锁失败，请人工处理, 解锁密码：9999");
            return true;
        }

        return false;
    }

    /**
     * 开启单体自动门
     *
     * @param rgv
     * @param taskDB
     * @param startNode
     * @param endNode
     */
    private static void openSingleAutoDoor(Rgv rgv, TaskDB taskDB, Node startNode, Node endNode) throws IOException {
        long sendTime3 = System.currentTimeMillis();
        PlcOperImpl systemOperOpen;
        List<SingleAutoDoor> automaticDoorOpers = new ArrayList<>();
        List<TurnoverOper> turnoverOpers = new ArrayList<>();
        List<Node> autoDoors = getDoorNodes(startNode, endNode);
        while (!rgv.getCurLocation().equals(startNode)) {
            rgv.checkInterrupt("等待小车到达门前", sendTime3);
            log.info(taskDB.getTaskId() + "任务等待小车到达门前");
            try {
                Thread.sleep(1000);
            } catch (Exception e) {
            }
        }
        // 终点存在且是自动门
        for (Node autoDoor : autoDoors) {
            SingleAutoDoor singleAutoDoor = (SingleAutoDoor) Plc.deviceAutomaticDoorCoordMap
                    .get(autoDoor);
            TurnoverOper turnoverOper = (TurnoverOper) Plc.deviceTurnoverMachineOperCoordMap.get(autoDoor);

            if (singleAutoDoor == null && turnoverOper == null) {
                throw new RuntimeException("翻转机自动门设备未配置或离线，不能行驶");
            }

            if (singleAutoDoor != null) {
                while (!singleAutoDoor.isOpen()) {
                    rgv.checkInterrupt("自动门未开启", sendTime3);
                    singleAutoDoor.setDeviceStatus("门未开启，" + rgv.getRgvName() + "无法通过，请开启门");
                    WcsFuncUtil.delay(1000);
                }
                while (singleAutoDoor.isLock()) {
                    rgv.checkInterrupt("自动门被锁定", sendTime3);
                    singleAutoDoor.setDeviceStatus("被锁定，等待解锁，如果长时间未解锁，请人工确认后解锁，解锁密码：9999");
                    WcsFuncUtil.delay(1000);
                }
                singleAutoDoor.setDeviceStatus("");
                automaticDoorOpers.add(singleAutoDoor);
            }
            if (turnoverOper != null) {
                while (!turnoverOper.isLowerLimit()) {
                    rgv.checkInterrupt("翻转机未水平", sendTime3);
                    turnoverOper.setDeviceStatus("开启，" + rgv.getRgvName() + "无法通过，翻转机需要水平");
                    WcsFuncUtil.delay(1000);
                }
                while (turnoverOper.isLock()) {
                    rgv.checkInterrupt("翻转机被锁定", sendTime3);
                    turnoverOper.setDeviceStatus("被锁定，等待解锁，如果长时间未解锁，请人工确认后解锁，解锁密码：9999");
                    WcsFuncUtil.delay(1000);
                }
                turnoverOper.setDeviceStatus("");
                turnoverOpers.add(turnoverOper);
            }
            if (turnoverOper != null) {
                systemOperOpen = PlcOperImpl.getPlcOper(turnoverOper.getPlc());
                rgv.setStatusDescription("锁定 " + turnoverOper.getDeviceName());
                systemOperOpen.lockDevice(turnoverOper.getDeviceNo(), rgv.getRgvDB().getRgvNo(), true);
                WcsFuncUtil.delay(100);
                systemOperOpen.getPlc().refreshCache();
            }
            if (singleAutoDoor != null) {
                systemOperOpen = PlcOperImpl.getPlcOper(singleAutoDoor.getPlc());
                rgv.setStatusDescription("锁定 " + singleAutoDoor.getDeviceName());
                systemOperOpen.lockDevice(singleAutoDoor.getDeviceNo(), rgv.getRgvDB().getRgvNo(), true);
                WcsFuncUtil.delay(100);
                systemOperOpen.getPlc().refreshCache();
            }
            rgv.setStatusDescription("");
            WcsFuncUtil.delay(2000);
        }
        for (SingleAutoDoor singleAutoDoor : automaticDoorOpers) {
            int waitTime = 0;
            systemOperOpen = PlcOperImpl.getPlcOper(singleAutoDoor.getPlc());
            while (!singleAutoDoor.isLock()) {
                rgv.checkInterrupt("等待自动门上锁成功", sendTime3);
                WcsFuncUtil.delay(2000);
                waitTime++;
                if (waitTime > 20) {
                    waitTime = 0;
                    log.info(taskDB.getTaskId() + "任务，等待门" + singleAutoDoor.getDeviceNo() + "超时未锁定，重发加锁");
                    singleAutoDoor.setDeviceStatus("锁定失败，重新加锁");
                    systemOperOpen.lockDevice(singleAutoDoor.getDeviceNo(), rgv.getRgvDB().getRgvNo(), true);
                }
            }
            singleAutoDoor.setDeviceStatus("");
        }
        for (TurnoverOper turnoverOper : turnoverOpers) {
            int waitTime = 0;
            systemOperOpen = PlcOperImpl.getPlcOper(turnoverOper.getPlc());
            while (!turnoverOper.isLock()) {
                rgv.checkInterrupt("等待翻转机上锁成功", sendTime3);
                WcsFuncUtil.delay(2000);
                waitTime++;
                if (waitTime > 20) {
                    waitTime = 0;
                    log.info(taskDB.getTaskId() + "任务,等待门" + turnoverOper.getDeviceNo() + "超时未开，重发开门");
                    turnoverOper.setDeviceStatus("锁定失败，重新加锁");
                    systemOperOpen.lockDevice(turnoverOper.getDeviceNo(), rgv.getRgvDB().getRgvNo(), true);
                }
            }
            turnoverOper.setDeviceStatus("");
        }
    }

    /**
     * 开启普通自动门
     *
     * @param rgv
     * @param taskDB
     * @param startNode
     * @param endNode
     * @throws IOException
     */
    private static void openAutoDoorNormal(Rgv rgv, TaskDB taskDB, Node startNode, Node endNode) throws IOException {
        long sendTime3 = System.currentTimeMillis();
        List<AutomaticDoorOper> automaticDoorOpers = new ArrayList<>();
        List<Node> autoDoors = getDoorNodes(startNode, endNode);
        // 终点存在且是自动门
        for (Node autoDoor : autoDoors) {
            openTheDoor(taskDB, rgv, startNode, sendTime3, automaticDoorOpers, autoDoor);
        }
        for (AutomaticDoorOper automaticDoorOper : automaticDoorOpers) {
            waitTheDoorOpen(taskDB, rgv, sendTime3, automaticDoorOper);
        }
    }

    /**
     * 关闭普通自动门
     *
     * @param rgv
     * @param taskDB
     * @param startNode
     * @param endNode
     * @throws IOException
     */
    private static void closeAutoDoorNormal(Rgv rgv, TaskDB taskDB, Node startNode, Node endNode) throws IOException {
        long sendTime4 = System.currentTimeMillis();
        List<AutomaticDoorOper> automaticDoorOpersClose = new ArrayList<>();
        List<Node> autoDoors = getDoorNodes(startNode, endNode);
        // 终点存在且是自动门
        for (Node autoDoor : autoDoors) {
            closeTheDoor(taskDB, rgv, endNode, sendTime4, automaticDoorOpersClose, autoDoor);
        }
        if (automaticDoorOpersClose.size() > 0) {
            waitTheDoorClose(taskDB, rgv, sendTime4, automaticDoorOpersClose);
        }
    }

    /**
     * 检测是否关闭成功，未成功重发
     *
     * @param taskDB
     * @param rgv
     * @param sendTime4
     * @param automaticDoorOpersClose
     */
    private static void waitTheDoorClose(TaskDB taskDB, Rgv rgv, long sendTime4, List<AutomaticDoorOper> automaticDoorOpersClose) {
        new Thread(() -> {
            for (AutomaticDoorOper automaticDoorOper : automaticDoorOpersClose) {
                int waitTime = 0;
                int count = Plc.pushPLCAutomaticDoor(String.valueOf(automaticDoorOper.getDeviceNo()), "RGV" + taskDB.getTaskId());
                if (count <= 0) {
                    try {
                        while (!automaticDoorOper.isClose()) {
                            rgv.checkInterrupt("ActionHandle10", sendTime4);
                            PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
                            try {
                                Thread.sleep(2000);
                            } catch (Exception e) {
                            }
                            waitTime++;
                            if (waitTime > 20) {
                                waitTime = 0;
                                log.info(taskDB.getTaskId() + "任务等待门" + automaticDoorOper.getDeviceNo() + "超时未关，重发关门");
                                try {
                                    systemOperClose.automaticDoor(automaticDoorOper.getDeviceNo(), false);
                                } catch (IOException e) {
                                    log.error("通讯异常,自动门操作失败!", e);
                                }
                            }
                        }
                    } finally {
                        LockNodeManager.releaseDeviceLock(rgv);
                    }
                }
            }
        });
    }

    /**
     * 关闭自动门
     *
     * @param taskDB
     * @param rgv
     * @param endNode
     * @param sendTime4
     * @param automaticDoorOpersClose
     * @param autoDoor
     * @throws IOException
     */
    private static void closeTheDoor(TaskDB taskDB, Rgv rgv, Node endNode, long sendTime4, List<AutomaticDoorOper> automaticDoorOpersClose, Node autoDoor) throws IOException {
        while (!rgv.getCurLocation().equals(endNode)) {
            rgv.checkInterrupt("ActionHandle8", sendTime4);
            log.info(taskDB.getTaskId() + "任务等待小车到达门后");
            try {
                Thread.sleep(1000);
            } catch (Exception e) {
            }
        }
        AutomaticDoorOper automaticDoorOper = (AutomaticDoorOper) Plc.deviceBaseCoordMap.get(new Coord(autoDoor));
        if (automaticDoorOper != null) {
            automaticDoorOpersClose.add(automaticDoorOper);
        }
        PlcOperImpl systemOperClose = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
        int count = Plc.pushPLCAutomaticDoor(String.valueOf(automaticDoorOper.getDeviceNo()), "RGV" + taskDB.getTaskId());
        if (count <= 0) {
            systemOperClose.automaticDoor(automaticDoorOper.getDeviceNo(), false);
        }
    }

    /**
     * 检测自动门开启
     *
     * @param taskDB
     * @param rgv
     * @param sendTime3
     * @param automaticDoorOper
     * @throws IOException
     */
    private static void waitTheDoorOpen(TaskDB taskDB, Rgv rgv, long sendTime3, AutomaticDoorOper automaticDoorOper) throws IOException {
        PlcOperImpl systemOperOpen;
        int waitTime = 0;
        systemOperOpen = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
        while (!automaticDoorOper.isOpen() && !automaticDoorOper.getPlc().getPlcCache().isTimeOut()) {
            rgv.checkInterrupt("ActionHandle8", sendTime3);
            try {
                Thread.sleep(2000);
            } catch (Exception e) {
            }
            waitTime++;
            if (waitTime > 20) {
                waitTime = 0;
                log.info(taskDB.getTaskId() + "任务等待门" + automaticDoorOper.getDeviceNo() + "超时未开，重发开门");
                Plc.putPLCAutomaticDoor(String.valueOf(automaticDoorOper.getDeviceNo()), "RGV" + taskDB.getTaskId());
                systemOperOpen.automaticDoor(automaticDoorOper.getDeviceNo(), true);
            }
        }
    }

    /**
     * 小车到位打开自动门
     *
     * @param taskDB
     * @param rgv
     * @param startNode
     * @param sendTime3
     * @param automaticDoorOpers
     * @param autoDoor
     * @throws IOException
     */
    private static void openTheDoor(TaskDB taskDB, Rgv rgv, Node startNode, long sendTime3, List<AutomaticDoorOper> automaticDoorOpers, Node autoDoor) throws IOException {
        PlcOperImpl systemOperOpen;
//        while (!rgv.getCurLocation().equals(startNode)) {
//            rgv.checkInterrupt("ActionHandle6", sendTime3);
//            log.info(taskDB.getTaskId() + "任务等待小车到达门前");
//            try {
//                Thread.sleep(1000);
//            } catch (Exception e) {
//            }
//        }
        AutomaticDoorOper automaticDoorOper = (AutomaticDoorOper) Plc.deviceBaseCoordMap.get(new Coord(autoDoor));
        if (automaticDoorOper == null) {
            log.error("未找到坐标:" + autoDoor.toString() + "的设备");
            throw new RuntimeException("未找到坐标:" + autoDoor.toString() + "的设备");
        }
        automaticDoorOpers.add(automaticDoorOper);
        systemOperOpen = PlcOperImpl.getPlcOper(automaticDoorOper.getPlc());
        Plc.putPLCAutomaticDoor(String.valueOf(automaticDoorOper.getDeviceNo()), "RGV" + taskDB.getTaskId());
        systemOperOpen.automaticDoor(automaticDoorOper.getDeviceNo(), true);
        try {
            Thread.sleep(2000);
        } catch (Exception e) {
        }
    }

    /**
     * 更新相应的货位状态为无货
     *
     * @param rgvNode
     * @param startNodedb
     * @throws IOException
     */
    private static void updateNodeStatusN(Node rgvNode, NodeDB startNodedb) throws IOException {
        if (!startNodedb.getGridType().equals(NodeType.CONNECTION)) {
            nodedbServiceImpl.updateNodeDBStatus(rgvNode, NodeStatus.N.toString());
        }
        if (startNodedb != null && startNodedb.getGridType() == NodeType.CONNECTION) {
            Coord deviceCoord = new Coord(startNodedb.getGridx(), startNodedb.getGridy(), startNodedb.getGridz());
            if (Plc.deviceLifterCoordMap.get(new Coord(deviceCoord.getX(), deviceCoord.getY(), 1)) != null) {
                if (PlcUnit.getLifterOper(deviceCoord).getPlcDeviceType() == PlcDeviceType.AUTOMATIC_LIFTER) {
                    PlcUnit.writeTwoDirectionRgvCargoState(deviceCoord, CargoState.No_CARGO);
                }
            }
        }
    }

    /**
     * 更新接驳点节点状态
     *
     * @param rgvNode
     * @param endNodedb
     * @throws IOException
     */
    private static void updateNodeStatusY(Node rgvNode, NodeDB endNodedb) throws IOException {
        if (endNodedb.getGridType().equals(NodeType.P) || endNodedb.getGridType().equals(NodeType.CONNECTION)) {
            nodedbServiceImpl.updateNodeDBStatus(rgvNode, NodeStatus.Y.toString());
        }
        //自卸式提升机接驳位置放货
        if (endNodedb != null && (endNodedb.getGridType() == NodeType.CONNECTION)) {
            Coord deviceCoord = new Coord(rgvNode.getX(), rgvNode.getY(), rgvNode.getZ());
            if (Plc.deviceLifterCoordMap.get(new Coord(deviceCoord.getX(), deviceCoord.getY(), 1)) != null) {
                if (PlcUnit.getLifterOper(deviceCoord).getPlcDeviceType() == PlcDeviceType.AUTOMATIC_LIFTER) {
                    PlcUnit.writeTwoDirectionRgvCargoState(deviceCoord, CargoState.HAS_CARGO);
                }
            }
        }
        // 如果是提升机左右，则需要更新是否有货
        if (endNodedb != null) {
            nodedbServiceImpl.updateLifterLeftOrRightNodeDB(rgvNode, NodeStatus.Y.toString());
        }
    }

    /**
     * 校验小车放货到接驳点之后光电是否照到，未照到抛出相应的异常
     *
     * @param taskDB
     * @param rgvNode
     * @param endNodedb
     */
    private static void checkPutPalletOk(TaskDB taskDB, Node rgvNode, NodeDB endNodedb) {
        if (endNodedb != null && (endNodedb.getGridType() == NodeType.CONNECTION
                || endNodedb.getGridType() == NodeType.ST || endNodedb.getGridType() == NodeType.L)) {
            Coord deviceCoord = new Coord(rgvNode.getX(), rgvNode.getY(), rgvNode.getZ());
            boolean canget = nodedbServiceImpl.isHaveCargo(deviceCoord);
            log.info(String.format("任务：%s已到达终点%s，所对应工位是否有货?%s", taskDB.getTaskId(), rgvNode.toString(), canget));
            // 小车对应的提升机接驳点无货，任务报错处理
            //自卸式提升机接驳点位置上没有光电检测
            Node node = nodedbServiceImpl.getNode(new Node(deviceCoord.getX(), deviceCoord.getY(), deviceCoord.getZ()));
            if (node.getNodeType() == NodeType.CONNECTION) {
                Coord coord = new Coord(node.getX(), node.getY(), 1);
                if (Plc.deviceLifterCoordMap.get(coord) != null) {
                    if (PlcUnit.getLifterOper(coord) instanceof TwoDirectionRgvLifterOper) {
                        canget = true;
                    }
                }
            }
            // 小车对应的提升机接驳点无货，任务报错处理
            if (!canget) {
                String errorMsg = String.format("任务%s出库异常，小车托板下降完成，任务终点%s所在工位是否有货?%s", taskDB.getTaskId(), rgvNode.toString(), canget);
                throw new PalletDownException(errorMsg);
            }
        }
    }

    /**
     * 判断小车是否可去其他层
     *
     * @param rgv
     * @param oneFloorForMorRgv
     */
    private static void checkRgvChangeFloor(Rgv rgv, boolean oneFloorForMorRgv) {
        if (!oneFloorForMorRgv) {
            // 添加换层时的提升机验证，验证任务目标层是否有车
            checkRgvEndFloor(rgv);
            Set<Floor> floorSet = new HashSet<>();
            floorSet.add(new Floor(parseGrid(rgv.getCurTask().getEndNodeStr()).getZ()));
            while (LockFloorManager.checkFloorsLocked(floorSet, rgv)) {
                long sendTime = System.currentTimeMillis();
                rgv.checkInterrupt("ActionHandleCheckRgvEndFloor", sendTime);
                try {
                    Thread.sleep(1000);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
            }
            // 给层加锁
            LockFloorManager.addLockedFloors(floorSet, rgv);
        }
    }

    /**
     * 检测提升机是否可锁定，如果任务目的点是提升机则持续等待，不是则重新规划路线
     *
     * @param taskDB
     * @param rgv
     * @param startNode
     * @param endNode
     * @param hoistOper
     * @param lifter
     */
    private static void checkLifterIsCanLock(TaskDB taskDB, Rgv rgv, Node startNode, Node endNode, LifterOper hoistOper, Lifter lifter) {
        while ((hoistOper.isPalletExisted(endNode.getZ(), Location.INSIDE) && rgv.isHasCargo()) || LockNodeManager.checkLifterLocked(lifter, rgv)) {
            long sendTime = System.currentTimeMillis();
            while (!((rgv.getRgvStatus() == RgvStatus.NODE_STANDBY || rgv.getRgvStatus() == RgvStatus.READY) && startNode.equals(rgv.getCurLocation()))) {
                log.info("{}请求{}，当前位置：{}，状态：{}，未在指定位置：{}或状态不在待命中/就绪", rgv.getRgvName(), hoistOper.getDeviceName(), rgv.getCurLocation(), rgv.getRgvStatus(), startNode);
                rgv.checkInterrupt("ActionHandle1", sendTime);
                try {
                    Thread.sleep(1000);
                } catch (InterruptedException e) {
                    log.error("请求提升机，线程休眠失败！{}，{}", rgv.getRgvName(), hoistOper.getDeviceName());
                }
            }
            if (!taskDB.selectEndNode().equals(new Node(lifter.getX(), lifter.getY(), endNode.getZ()))) {
                try {
                    Thread.sleep(5000);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
                throw new OccupyLifterFailException(String.format("%s占用%s失败！", rgv.getRgvName(), hoistOper.getDeviceName()));
            }
            try {
                Thread.sleep(2000);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
    }

    /**
     * 同层不能多车情况下校验
     *
     * @param rgv
     */
    public static void checkRgvEndFloor(Rgv rgv) {
        long sendTime = System.currentTimeMillis();
        //检验目的层是否有车，有车则等待调用提升机
        boolean flag = true;
        while (flag) {
            List<Rgv> rgvList = SystemInit.getCanRunRgvListWithoutDisable();
            rgv.checkInterrupt("ActionHandle5", sendTime);
            for (Rgv rgvTemp : rgvList) {
                if (rgvTemp.getCurLocation().getZ() == rgv.getCurTask().selectEndNode().getZ() &&
                        !rgvTemp.getRgvDB().getRgvNo().equals(rgv.getRgvDB().getRgvNo())) {
                    try {
                        Thread.sleep(1000);
                        log.info("当前系统不允许同层多车，小车目的层有车，等待中...");
                        break;
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                }
            }
            flag = false;
        }
    }

    /**
     * 根据路径的起点和终点获取路径中的自动门
     *
     * @param start
     * @param end
     * @return
     */
    private static List<Node> getDoorNodes(Node start, Node end) {
        List<Node> passNode = new ArrayList<>();
        List<Node> doors = new ArrayList<>();
        // 自动门在Y轴
        if (start.getX() == end.getX()) {
            int count = Math.abs(start.getY() - end.getY());
            if (start.getY() > end.getY()) {
                for (int i = 0; i < count; i++) {
                    passNode.add(new Node(start.getX(), start.getY() - (i + 1), start.getZ()));
                }
            }
            if (start.getY() < end.getY()) {
                for (int i = 0; i < count; i++) {
                    passNode.add(new Node(start.getX(), start.getY() + (i + 1), start.getZ()));
                }
            }

        } else if (start.getY() == end.getY()) { // 自动门在X轴
            int count = Math.abs(start.getX() - end.getX());
            if (start.getX() > end.getX()) {
                for (int i = 0; i < count; i++) {
                    passNode.add(new Node(start.getX() - (i + 1), start.getY(), start.getZ()));
                }
            }
            if (start.getX() < end.getX()) {
                for (int i = 0; i < count; i++) {
                    passNode.add(new Node(start.getX() + (i + 1), start.getY(), start.getZ()));
                }
            }
        }

        for (Node pn : passNode) {
            NodeDB nodeDB = nodedbServiceImpl.getNodeDB(pn);
            if (nodeDB != null && nodeDB.getIsDoor() != null && nodeDB.getIsDoor()) {
                doors.add(pn);
            }
        }

        return doors;
    }

}
